#include "Encoder.h"
#include "lpf.h"
#include "stm32g4xx_ll_dma.h"
#include "stm32g4xx_ll_usart.h"
#include "motor_driver.h"
Encoder_t Encoder =
    {
        .Enable = 1,
        .Offset = 0,
        .RotorDirection = 1,
        .SpeedSwitch = 0,

#if (ENCODER_TYPE == SingleJinGangEncoder)
        .resolution_pos = 16,
        .scale_pos = 65536,
        .resolution_vel = 16,
        .scale_vel = 65536,
        .tx_buf = {0x73},
#elif (ENCODER_TYPE == DoubleGongWangEncoder)
#define ENCODER_RESOLUTION (18)
#define ENCODER_MULTI_SHIFT (20 - ENCODER_RESOLUTION)
        .resolution_pos = ENCODER_RESOLUTION,
        .scale_pos = (1<<ENCODER_RESOLUTION),
        .resolution_vel = 16,
        .scale_vel = 65536,
        .tx_buf = {0x1A},
#elif (ENCODER_TYPE == DoubleJinGangEncoder)
#define ENCODER_RESOLUTION (18)
#define ENCODER_MULTI_SHIFT (24 - ENCODER_RESOLUTION)
        .resolution_pos = ENCODER_RESOLUTION,
        .scale_pos = (1<<ENCODER_RESOLUTION),
        .resolution_vel = 16,
        .scale_vel = 65536,
        .tx_buf = {0x33},
#else
#error "encoder type must be specified ! 必须指定编码器类型！";
#endif
};

#define ENCODER_SINGLE_SHIFT (ENCODER_RESOLUTION)



Filter_t FilterCSP =
{
  .fSpeedFilterFreq = 1000,
};

const uint8_t crcTable[256] = {
    0x00, 0x97, 0xB9, 0x2E, 0xE5, 0x72, 0x5C, 0xCB, 0x5D, 0xCA, 0xE4, 0x73, 0xB8, 0x2F, 0x01, 0x96, 0xBA, 0x2D, 0x03, 0x94,
    0x5F, 0xC8, 0xE6, 0x71, 0xE7, 0x70, 0x5E, 0xC9, 0x02, 0x95, 0xBB, 0x2C, 0xE3, 0x74, 0x5A, 0xCD, 0x06, 0x91, 0xBF, 0x28, 0xBE,
    0x29, 0x07, 0x90, 0x5B, 0xCC, 0xE2, 0x75, 0x59, 0xCE, 0xE0, 0x77, 0xBC, 0x2B, 0x05, 0x92, 0x04, 0x93, 0xBD, 0x2A, 0xE1, 0x76,
    0x58, 0xCF, 0x51, 0xC6, 0xE8, 0x7F, 0xB4, 0x23, 0x0D, 0x9A, 0x0C, 0x9B, 0xB5, 0x22, 0xE9, 0x7E, 0x50, 0xC7, 0xEB, 0x7C, 0x52,
    0xC5, 0x0E, 0x99, 0xB7, 0x20, 0xB6, 0x21, 0x0F, 0x98, 0x53, 0xC4, 0xEA, 0x7D, 0xB2, 0x25, 0x0B, 0x9C, 0x57, 0xC0, 0xEE, 0x79,
    0xEF, 0x78, 0x56, 0xC1, 0x0A, 0x9D, 0xB3, 0x24, 0x08, 0x9F, 0xB1, 0x26, 0xED, 0x7A, 0x54, 0xC3, 0x55, 0xC2, 0xEC, 0x7B, 0xB0,
    0x27, 0x09, 0x9E, 0xA2, 0x35, 0x1B, 0x8C, 0x47, 0xD0, 0xFE, 0x69, 0xFF, 0x68, 0x46, 0xD1, 0x1A, 0x8D, 0xA3, 0x34, 0x18, 0x8F,
    0xA1, 0x36, 0xFD, 0x6A, 0x44, 0xD3, 0x45, 0xD2, 0xFC, 0x6B, 0xA0, 0x37, 0x19, 0x8E, 0x41, 0xD6, 0xF8, 0x6F, 0xA4, 0x33, 0x1D,
    0x8A, 0x1C, 0x8B, 0xA5, 0x32, 0xF9, 0x6E, 0x40, 0xD7, 0xFB, 0x6C, 0x42, 0xD5, 0x1E, 0x89, 0xA7, 0x30, 0xA6, 0x31, 0x1F, 0x88,
    0x43, 0xD4, 0xFA, 0x6D, 0xF3, 0x64, 0x4A, 0xDD, 0x16, 0x81, 0xAF, 0x38, 0xAE, 0x39, 0x17, 0x80, 0x4B, 0xDC, 0xF2, 0x65, 0x49,
    0xDE, 0xF0, 0x67, 0xAC, 0x3B, 0x15, 0x82, 0x14, 0x83, 0xAD, 0x3A, 0xF1, 0x66, 0x48, 0xDF, 0x10, 0x87, 0xA9, 0x3E, 0xF5, 0x62,
    0x4C, 0xDB, 0x4D, 0xDA, 0xF4, 0x63, 0xA8, 0x3F, 0x11, 0x86, 0xAA, 0x3D, 0x13, 0x84, 0x4F, 0xD8, 0xF6, 0x61, 0xF7, 0x60, 0x4E,
    0xD9, 0x12, 0x85, 0xAB, 0x3C};

void ENC_SetMecAngle(ENCODER_Handle_t *pHandle, int16_t hMecAngle)
{
    int32_t wTemp;
    wTemp = Encoder.OrigMecAngle17b / 2;
    wTemp &= 0xFFFF;
    Encoder.Offset = -wTemp;
}

uint8_t calcCRC(uint8_t *buffer, uint8_t length)
{
    uint8_t temp = *buffer++;
    while (--length)
    {
        temp = *buffer++ ^ crcTable[temp];
    }
    return crcTable[temp];
}

/*算位置*/
int32_t OriInPos17b = 0, OriInPos24b = 0, OriOutPos20b = 0, OriOutPos24b = 0;
int32_t LastOriInPos17b = 0, LastOriInPos24b = 0, LastOriOutPos20b = 0, LastOriOutPos24b = 0;
/*算圈数*/
int32_t InPos1 = 0, InPos2 = 0, TempIncircle = 0;
int32_t OutPos1 = 0, OutPos2 = 0, TempOutcircle = 0;
uint16_t t = 0;
/*算速度*/
int32_t SelfPos = 0, SelfLastPos = 0, SelfPosError = 0, SelfPosCnt = 0, SelfPosIndex[100];
float SelfSpeed = 0;
float x = 12;
int32_t tt;
int32_t SelfOutPos = 0, SelfLastOutPos = 0, SelfOutPosError = 0, SelfOutPosCnt = 0, SelfOutPosIndex[2];

EncObserver_t EncObserver;

void encoder_on_datarecive(void)
{
    int32_t wTemp;
    int32_t wPosition;
    Encoder.Rxcnt++;
#if (ENCODER_TYPE == SingleJinGangEncoder)
    {
        Encoder.Crc8 = Encoder.rx_buf[7];
        Encoder.Crc8Result = calcCRC(Encoder.rx_buf, 7);
        if (Encoder.Crc8Result != Encoder.Crc8)
        {
            Encoder.Status = 0;
        }
        else
        {
					debugget.encoder_rxcnt++;
            Encoder.RxCrccnt++;
            Encoder.InNumOfCircle = (Encoder.rx_buf[0] << 8) + Encoder.rx_buf[1];
            Encoder.OrigMecAngle17b = (Encoder.rx_buf[2] << 16) + (Encoder.rx_buf[3] << 8) + Encoder.rx_buf[4];
            Encoder.OrigSpeed = (Encoder.rx_buf[5] << 8) + Encoder.rx_buf[6];
            Encoder.Status = 1;
            Encoder.Recieved = 1;
            Encoder.NoRecieved = 0;

            wPosition = (Encoder.OrigMecAngle17b / 2) + Encoder.Offset;
            wPosition += Encoder.InNumOfCircle << 16;
            Encoder.InOriPosition = wPosition;
//            Encoder.InPosition = wPosition - (*SYS_Variable.ZeroPositionOffset);
            Encoder.OutPosition = Encoder.InPosition;
            Encoder.Position = Encoder.InPosition;
        }
    }
#elif (ENCODER_TYPE == DoubleGongWangEncoder)
    {
        if (Encoder.rx_buf[0] == 0x1A && Encoder.rx_buf[5] == 0x14)
        {
            Encoder.Status = 1;
            Encoder.Recieved = 1;
            Encoder.NoRecieved = 0;
            OriInPos17b = Encoder.rx_buf[2] + (Encoder.rx_buf[3] << 8) + (Encoder.rx_buf[4] << 16);
            OriOutPos20b = (Encoder.rx_buf[6] + (Encoder.rx_buf[7] << 8) + (Encoder.rx_buf[8] << 16));

            Encoder.LastOrigMecAngle17b = Encoder.OrigMecAngle17b;
            Encoder.OrigMecAngle17b = OriInPos17b >> 0;
            Encoder.LastOrigOutMecAngle20b = Encoder.OrigOutMecAngle20b;
            Encoder.OrigOutMecAngle20b = OriOutPos20b;
            if (++t > 10) // 给LastOrigMecAngle17b和LastOrigOutMecAngle20b赋值不然初始化圈数为-1
            {
                t = 100;
                /*算内圈圈数和位置*/
                InPos1 = (Encoder.LastOrigMecAngle17b >> 15) & 0x03;
                InPos2 = (Encoder.OrigMecAngle17b >> 11) & 0x30;
                TempIncircle = InPos1 | InPos2;
                if (TempIncircle == 0x03)
                    Encoder.InNumOfCircle++;
                else if (TempIncircle == 0x30)
                    Encoder.InNumOfCircle--;
                wPosition = (Encoder.OrigMecAngle17b / 2) + Encoder.Offset;
                wPosition += Encoder.InNumOfCircle << 16;
                Encoder.InOriPosition = wPosition;
                Encoder.InPosition = wPosition;

                /*算内圈速度*/
                SelfLastPos = SelfPos;
                SelfPos = Encoder.InPosition;
                SelfPosError = SelfPos - SelfLastPos;
                if (SelfPosCnt >= 32)
                {
                    SelfPosCnt = 0;
                }
                SelfPosIndex[SelfPosCnt] = SelfPosError;
                SelfPosCnt++;
                SelfSpeed = SelfPosIndex[0] + SelfPosIndex[1] + SelfPosIndex[2] + SelfPosIndex[3] + SelfPosIndex[4] +
                            SelfPosIndex[5] + SelfPosIndex[6] + SelfPosIndex[7] + SelfPosIndex[8] + SelfPosIndex[9] +
                            SelfPosIndex[10] + SelfPosIndex[11] + SelfPosIndex[12] + SelfPosIndex[13] + SelfPosIndex[14] +
                            SelfPosIndex[15] + SelfPosIndex[16] + SelfPosIndex[17] + SelfPosIndex[18] + SelfPosIndex[19] +
                            SelfPosIndex[20] + SelfPosIndex[21] + SelfPosIndex[22] + SelfPosIndex[23] + SelfPosIndex[24] +
                            SelfPosIndex[25] + SelfPosIndex[26] + SelfPosIndex[27] + SelfPosIndex[28] + SelfPosIndex[29] +
                            SelfPosIndex[30] + SelfPosIndex[31];
                Encoder.OrigSpeed = (int16_t)SelfSpeed / 13.1072; // 65536*TarSpeed *0.01/pwm_freq * 32 / 100
                /*算外圈圈数和位置*/
                OutPos1 = (Encoder.LastOrigOutMecAngle20b >> 18) & 0x03;
                OutPos2 = (Encoder.OrigOutMecAngle20b >> 14) & 0x30;
                TempOutcircle = OutPos1 | OutPos2;
                if (TempOutcircle == 0x03)
                    Encoder.OutNumOfCircle++;
                else if (TempOutcircle == 0x30)
                    Encoder.OutNumOfCircle--;
                
                wPosition = (Encoder.OrigOutMecAngle20b) >> ENCODER_MULTI_SHIFT;
                wPosition += Encoder.OutNumOfCircle << ENCODER_SINGLE_SHIFT;
                Encoder.OutPosition = wPosition;
                Encoder.Position = Encoder.OutPosition - (*SYS_Variable.ZeroPositionOffset);
                //                if (Encoder.Position < -131072)
                //                {
                //                    Encoder.Position += 262144;
                //                }
                // Encoder.OutPosition = (Encoder.OrigOutMecAngle20b >> 2) - (*SYS_Variable.ZeroPositionOffset);
                // Encoder.Position = Encoder.OutPosition;
                // while (Encoder.Position > 131072)
                //     Encoder.Position -= 262144;
                //   while(Encoder.Position < -131072)
                //     Encoder.Position += 262144;
            }
        }
    }
#elif (ENCODER_TYPE == DoubleJinGangEncoder)
    {
        Encoder.Crc8 = Encoder.rx_buf[6];
        Encoder.Crc8Result = calcCRC(Encoder.rx_buf, 6);
        if (Encoder.Crc8Result != Encoder.Crc8)
        {
            Encoder.Status = 0;
        }
        else
        {
            Encoder.Status = 1;
            Encoder.Recieved = 1;
            Encoder.NoRecieved = 0;
            Encoder.InSingleData = OriInPos24b = Encoder.rx_buf[3] + (Encoder.rx_buf[4] << 8) + (Encoder.rx_buf[5] << 16);
            Encoder.OutSingleData = OriOutPos24b = (Encoder.rx_buf[0] + (Encoder.rx_buf[1] << 8) + (Encoder.rx_buf[2] << 16));

            Encoder.LastOrigMecAngle17b = Encoder.OrigMecAngle17b;
            Encoder.OrigMecAngle17b = OriInPos24b >> 7;
            Encoder.LastOrigOutMecAngle20b = Encoder.OrigOutMecAngle20b;
            Encoder.OrigOutMecAngle20b = OriOutPos24b;
            if (++t > 10)
            {
                t = 100;
                /*算内圈圈数和位置*/
                InPos1 = (Encoder.LastOrigMecAngle17b >> 15) & 0x03;
                InPos2 = (Encoder.OrigMecAngle17b >> 11) & 0x30;
                TempIncircle = InPos1 | InPos2;
                if (TempIncircle == 0x03)
                    Encoder.InNumOfCircle++;
                else if (TempIncircle == 0x30)
                    Encoder.InNumOfCircle--;
                wPosition = (Encoder.OrigMecAngle17b / 2) + Encoder.Offset;
                wPosition += Encoder.InNumOfCircle << 16;
                Encoder.InOriPosition = wPosition;
                Encoder.InPosition = wPosition;
                /*算内圈速度*/
                SelfLastPos = SelfPos;
                SelfPos = Encoder.InPosition;
                SelfPosError = SelfPos - SelfLastPos;
                if (SelfPosCnt >= 32)
                {
                    SelfPosCnt = 0;
                }
                SelfPosIndex[SelfPosCnt] = SelfPosError;
                SelfPosCnt++;
                SelfSpeed = SelfPosIndex[0] + SelfPosIndex[1] + SelfPosIndex[2] + SelfPosIndex[3] + SelfPosIndex[4] +
                            SelfPosIndex[5] + SelfPosIndex[6] + SelfPosIndex[7] + SelfPosIndex[8] + SelfPosIndex[9] +
                            SelfPosIndex[10] + SelfPosIndex[11] + SelfPosIndex[12] + SelfPosIndex[13] + SelfPosIndex[14] +
                            SelfPosIndex[15] + SelfPosIndex[16] + SelfPosIndex[17] + SelfPosIndex[18] + SelfPosIndex[19] +
                            SelfPosIndex[20] + SelfPosIndex[21] + SelfPosIndex[22] + SelfPosIndex[23] + SelfPosIndex[24] +
                            SelfPosIndex[25] + SelfPosIndex[26] + SelfPosIndex[27] + SelfPosIndex[28] + SelfPosIndex[29] +
                            SelfPosIndex[30] + SelfPosIndex[31];
                Encoder.OrigSpeed = SelfSpeed / 13.1072; // 65536*TarSpeed *0.01/pwm_freq * 32 / 100
                /*算外圈圈数和位置*/

/*算外圈加减速度START*/
                SelfLastOutPos = SelfOutPos;
                SelfOutPos = Encoder.OutPosition;
                SelfOutPosError = SelfOutPos - SelfLastOutPos;
                  if (SelfOutPosCnt > 1)
                  {
                    SelfOutPosCnt = 0;
                  }
                  SelfOutPosIndex[SelfOutPosCnt] = SelfOutPosError;
                  if(++tt > 10)//给SelfOutPosIndex赋初始值
                  {
                    tt = 100; 
                    EncObserver.OutAccEcc = SelfOutPosIndex[1] - SelfOutPosIndex[0];
                    if(abs(EncObserver.OutAccEcc) > Encoder.scale_pos / 4)
                    {
                      Encoder.debug_data = Encoder.scale_pos;
                      MC_Interface.ErrorCode = EC_EncoderErr;
                    }
                    else
                    {
                        Encoder.debug_data = 0;
                    }
                  }
                  SelfOutPosCnt++;
                  /*算外圈加减速度END*/

                /*金钢双编暂时屏蔽多圈*/
                OutPos1 = (Encoder.LastOrigOutMecAngle20b >> 22) & 0x03;
                OutPos2 = (Encoder.OrigOutMecAngle20b >> 18) & 0x30;
                TempOutcircle = OutPos1 | OutPos2;
                if (TempOutcircle == 0x03)
                {
                  Encoder.OutNumOfCircle++;
                }
                else if (TempOutcircle == 0x30)
                {
                  Encoder.OutNumOfCircle--;
                }
                wPosition = (Encoder.OrigOutMecAngle20b >> ENCODER_MULTI_SHIFT);
                wPosition += Encoder.OutNumOfCircle << ENCODER_SINGLE_SHIFT;
                Encoder.OutPosition = wPosition;
                Encoder.Position = Encoder.OutPosition - (*SYS_Variable.ZeroPositionOffset);
                //                if (Encoder.Position < -131072)
                //                {
                //                    Encoder.Position += 262144;
                //                }

                // Encoder.OutPosition = (Encoder.OrigOutMecAngle20b >> 6) - (*SYS_Variable.ZeroPositionOffset);
                // Encoder.Position = Encoder.OutPosition;
                // while (Encoder.Position > 131072)
                //     Encoder.Position -= 262144;
                //   while(Encoder.Position < -131072)
                //     Encoder.Position += 262144;
                
            }
        }
    }
#endif
    /*速度滤波*/
    wTemp = Encoder.OrigSpeed * 10;
    wTemp += Encoder.OrigSpeedFilter * 19;
    Encoder.FloatOrigSpeedFilter = Encoder.FloatOrigSpeedFilter * 0.9f + Encoder.OrigSpeed;
    Encoder.OrigSpeedFilter = wTemp / 20;
		Encoder.ElectricalSpeed = Encoder.OrigSpeedFilter * kMotorPn * RPM_TO_RAD_PER_SEC;
    FilterCSP.fSpeedFilter1KHz = FilterCSP.fSpeedFilter1KHz * 0.95f;
    FilterCSP.fSpeedFilter1KHz += Encoder.OrigSpeed;
    Encoder.SpeedFilter1KHz = FilterCSP.fSpeedFilter1KHz / 2;
}


__weak bool SPD_IsMecSpeedReliable( SpeednPosFdbk_Handle_t * pHandle, int16_t * pMecSpeedUnit )
{
  bool SpeedSensorReliability = true;
  uint8_t bSpeedErrorNumber;
  uint8_t bMaximumSpeedErrorsNumber = pHandle->bMaximumSpeedErrorsNumber;

  bool SpeedError = false;
  uint16_t hAbsMecSpeedUnit, hAbsMecAccelUnitP;
  int16_t hAux;

  bSpeedErrorNumber = pHandle->bSpeedErrorNumber;

  /* Compute absoulte value of mechanical speed */
  if ( *pMecSpeedUnit < 0 )
  {
    hAux = -( *pMecSpeedUnit );
    hAbsMecSpeedUnit = ( uint16_t )( hAux );
  }
  else
  {
    hAbsMecSpeedUnit = ( uint16_t )( *pMecSpeedUnit );
  }

  if ( hAbsMecSpeedUnit > pHandle->hMaxReliableMecSpeedUnit )
  {
    SpeedError = true;
  }

  if ( hAbsMecSpeedUnit < pHandle->hMinReliableMecSpeedUnit )
  {
    SpeedError = true;
  }

  /* Compute absoulte value of mechanical acceleration */
  if ( pHandle->hMecAccelUnitP < 0 )
  {
    hAux = -( pHandle->hMecAccelUnitP );
    hAbsMecAccelUnitP = ( uint16_t )( hAux );
  }
  else
  {
    hAbsMecAccelUnitP = ( uint16_t )( pHandle->hMecAccelUnitP );
  }

  if ( hAbsMecAccelUnitP > pHandle->hMaxReliableMecAccelUnitP )
  {
    SpeedError = true;
  }

  if ( SpeedError == true )
  {
    if ( bSpeedErrorNumber < bMaximumSpeedErrorsNumber )
    {
      bSpeedErrorNumber++;
    }
  }
  else
  {
    if ( bSpeedErrorNumber < bMaximumSpeedErrorsNumber )
    {
      bSpeedErrorNumber = 0u;
    }
  }

  if ( bSpeedErrorNumber == bMaximumSpeedErrorsNumber )
  {
    SpeedSensorReliability = false;
  }

  pHandle->bSpeedErrorNumber = bSpeedErrorNumber;

  return ( SpeedSensorReliability );
}

bool ENC_CalcAvrgMecSpeedUnit(ENCODER_Handle_t *pHandle, int16_t *pMecSpeedUnit)
{
    TIM_TypeDef *TIMx = pHandle->TIMx;
    int32_t wOverallAngleVariation = 0;
    int32_t wtemp1;
    uint8_t bBufferIndex = 0u;
    bool bReliability = true;
    uint8_t bBufferSize = pHandle->SpeedBufferSize;
    uint32_t OverflowCntSample;
    uint32_t CntCapture;
    uint32_t directionSample;
    uint8_t OFbit = 0;

    /*Calculation of delta angle*/
    pHandle->DeltaCapturesBuffer[pHandle->DeltaCapturesIndex] = Encoder.InPosition - Encoder.PrevPosition;
    Encoder.PrevPosition = Encoder.InPosition;
    Encoder.Speed1 = pHandle->DeltaCapturesBuffer[pHandle->DeltaCapturesIndex] * 100000 / 65536 * 2; // rp100s 0.001Hz
    /*Computes & returns average mechanical speed */
    for (bBufferIndex = 0u; bBufferIndex < bBufferSize; bBufferIndex++)
    {
        wOverallAngleVariation += pHandle->DeltaCapturesBuffer[bBufferIndex];
    }
    wtemp1 = wOverallAngleVariation * SPEED_UNIT / 524.272; // 65536*TarSpeed *0.01/pwm_freq * 32 / 100
    Encoder.Speed = wtemp1;

    if (Encoder.SpeedSwitch == 0)
    {
        wtemp1 = Encoder.OrigSpeedFilter;
    }
    else if (Encoder.SpeedSwitch == 1)
    {
        wtemp1 = Encoder.SpeedFilter1KHz;
    }
    else if (Encoder.SpeedSwitch == 2)
    {
        wtemp1 = Encoder.Speed;
    }
    else if (Encoder.SpeedSwitch == 3)
    {
        wtemp1 = lpf_run(&lpf_speed, pHandle->DeltaCapturesBuffer[pHandle->DeltaCapturesIndex]) * SPEED_LOOP_FREQUENCY_HZ * SPEED_UNIT / 65536;
    }

    *pMecSpeedUnit = (int16_t)(wtemp1);
    //	*pMecSpeedUnit = ( int16_t )( Encoder.Speed );

    /*Computes & stores average mechanical acceleration */
    pHandle->_Super.hMecAccelUnitP = (int16_t)(wtemp1 -
                                               pHandle->_Super.hAvrMecSpeedUnit);

    /*Stores average mechanical speed */
    pHandle->_Super.hAvrMecSpeedUnit = (int16_t)wtemp1;

    /*Computes & stores the instantaneous electrical speed [dpp], var wtemp1*/
    wtemp1 = pHandle->DeltaCapturesBuffer[pHandle->DeltaCapturesIndex] *
             (int32_t)(pHandle->SpeedSamplingFreqHz) *
             (int32_t)pHandle->_Super.bElToMecRatio;
    wtemp1 /= (int32_t)(pHandle->PulseNumber);
    wtemp1 *= (int32_t)(pHandle->_Super.DPPConvFactor);
    wtemp1 /= (int32_t)(pHandle->_Super.hMeasurementFrequency);

    pHandle->_Super.hElSpeedDpp = (int16_t)wtemp1;

    /*last captured value update*/
    //  pHandle->PreviousCapture = CntCapture;
    /*Buffer index update*/
    pHandle->DeltaCapturesIndex++;

    if (pHandle->DeltaCapturesIndex == pHandle->SpeedBufferSize)
    {
        pHandle->DeltaCapturesIndex = 0u;
    }

    /*Checks the reliability status, then stores and returns it*/
    if (pHandle->TimerOverflowError)
    {
        bReliability = false;
        pHandle->SensorIsReliable = false;
        pHandle->_Super.bSpeedErrorNumber = pHandle->_Super.bMaximumSpeedErrorsNumber;
    }
    else
    {
        bReliability = SPD_IsMecSpeedReliable(&pHandle->_Super, pMecSpeedUnit);
    }

    return (bReliability);
}

int16_t ENC_CalcAngle(ENCODER_Handle_t *pHandle)
{

    int32_t wtemp1;
    int32_t position;
    int16_t elAngle;  /* s16degree format */
    int16_t mecAngle; /* s16degree format */
    float ftemp1;
    /*print control*/

    mecAngle = Encoder.InOriPosition & 0xFFFF;
    Encoder.MecAngle = mecAngle;

    int16_t hMecAnglePrev = pHandle->_Super.hMecAngle;
    pHandle->_Super.hMecAngle = mecAngle;
    /*Computes and stores the rotor electrical angle*/
    elAngle = mecAngle * pHandle->_Super.bElToMecRatio;
    pHandle->_Super.hElAngle = elAngle;
    int16_t hMecSpeedDpp = mecAngle - hMecAnglePrev;
    pHandle->_Super.wMecAngle += (int32_t)(hMecSpeedDpp);
    Encoder.ElAngle = elAngle;
    /*Returns rotor electrical angle*/
    return (elAngle);
}

#define ENCODER_RECIVE_INIT(rxbuffer)                                              \
    do                                                                             \
    {                                                                              \
        LL_USART_ClearFlag_IDLE(USART2);                                           \
        LL_USART_ClearFlag_TC(USART2);                                             \
        LL_USART_EnableDMAReq_RX(USART2);                                          \
        LL_USART_EnableDMAReq_TX(USART2);                                          \
        LL_DMA_SetPeriphAddress(DMA1, LL_DMA_CHANNEL_1, (uint32_t)(&USART2->RDR)); \
        LL_DMA_SetPeriphAddress(DMA1, LL_DMA_CHANNEL_2, (uint32_t)(&USART2->TDR)); \
        LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_1);                             \
        LL_DMA_SetMemoryAddress(DMA1, LL_DMA_CHANNEL_1, (uint32_t)rxbuffer);       \
        LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_1, sizeof(rxbuffer));            \
        LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_1);                              \
    } while (0)

#define ENCODER_TX(txbuffer, txlen)                                          \
    do                                                                       \
    {                                                                        \
        LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_2);                       \
        LL_DMA_ClearFlag_TC2(DMA1);                                          \
        LL_DMA_ClearFlag_TE2(DMA1);                                          \
        LL_DMA_SetMemoryAddress(DMA1, LL_DMA_CHANNEL_2, (uint32_t)txbuffer); \
        LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_2, txlen);                 \
        LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_2);                        \
    } while (0)

#define ENCODER_RX(rxbuffer, on_recive, on_no_data)                          \
    do                                                                       \
    {                                                                        \
        if (LL_USART_IsActiveFlag_FE(USART2))                                \
            LL_USART_ClearFlag_FE(USART2);                                   \
        if (LL_USART_IsActiveFlag_ORE(USART2))                               \
            LL_USART_ClearFlag_ORE(USART2);                                  \
        if (LL_USART_IsActiveFlag_NE(USART2))                                \
            LL_USART_ClearFlag_NE(USART2);                                   \
        if (LL_USART_IsActiveFlag_PE(USART2))                                \
            LL_USART_ClearFlag_PE(USART2);                                   \
        if (LL_USART_IsActiveFlag_CM(USART2))                                \
            LL_USART_ClearFlag_CM(USART2);                                   \
        if (LL_USART_IsActiveFlag_IDLE(USART2))                              \
            LL_USART_ClearFlag_IDLE(USART2);                                 \
        int32_t remain = LL_DMA_GetDataLength(DMA1, LL_DMA_CHANNEL_1);       \
        LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_1);                       \
        if (remain >= 0 && remain <= sizeof(rxbuffer))                       \
        {                                                                    \
            int32_t datalen = sizeof(rxbuffer) - remain;                     \
            if (datalen > 0)                                                 \
            {                                                                \
                on_recive;                                                   \
            }                                                                \
        }                                                                    \
        else                                                                 \
        {                                                                    \
            on_no_data;                                                      \
        }                                                                    \
        LL_DMA_SetMemoryAddress(DMA1, LL_DMA_CHANNEL_1, (uint32_t)rxbuffer); \
        LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_1, sizeof(rxbuffer));      \
        LL_DMA_ClearFlag_TE1(DMA1);                                          \
        LL_DMA_ClearFlag_TC1(DMA1);                                          \
        LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_1);                        \
    } while (0)

void encoder_init(void)
{
    ENCODER_RECIVE_INIT(Encoder.rx_buf);
}

void encoder_tx(void)
{
    ENCODER_TX(Encoder.tx_buf, 1);
    Encoder.Txcnt++;
    if (Encoder.Enable == 0)
    {
        Encoder.isRun = 0;
        // Encoder.NoRecieved = 0;
        return;
    }
    else
    {
        Encoder.isRun = 1;
    }
    if (Encoder.NoRecieved > 50)
    {
        Encoder.ErrorCode |= 4;
    }
    else
    {
        Encoder.ErrorCode &= ~4;
    }
    Encoder.NoRecieved++;
}

void encoder_rx(void)
{
    ENCODER_RX(Encoder.rx_buf, encoder_on_datarecive(), NULL);
}

void encoder_rx_noprocess(void)
{
    ENCODER_RX(Encoder.rx_buf, NULL, NULL);
}
